#include <stdio.h>
#include <math.h>
#include <string.h>

// 234,123
// 236.422671989273
// 121.00336289697373

static const double CoeffX[] = {7.41694501e-07, 1.96288715e-12, -2.54558139e-17, 8.10036042e-05, -8.12688762e-05};
static const double CoeffY[] = {4.34618963e-06, -9.48322433e-11, 6.70426052e-16, -4.33957677e-05, -1.07493407e-04};

static void correct(double x, double y, const double coeff[5], double *x_correct, double *y_correct)
{
    double k1, k2, k3, p1, p2;

    double r2 = x * x + y * y;

    // 径向畸变 radial distortion
    double x_rd = x * (1 + k1 * r2 + k2 * pow(r2, 2) + k3 * pow(r2, 3));
    double y_rd = y * (1 + k1 * r2 + k2 * pow(r2, 2) + k3 * pow(r2, 3));

    // 切向畸变 tangential distortion
    double x_td = 2 * p1 * x * y + p2 * (r2 + 2 * (x * x));
    double y_td = 2 * p2 * x * y + p1 * (r2 + 2 * (y * y));

    *x_correct = x_rd + x_td;
    *y_correct = y_rd + y_td;
}

static double __correctX(double x, double y, const double coefficients[])
{
    double k1, k2, k3, p1, p2;
    k1 = coefficients[0];
    k2 = coefficients[1];
    k3 = coefficients[2];
    p1 = coefficients[3];
    p2 = coefficients[4];

    double r2 = x * x + y * y;

    // 径向畸变 radial distortion
    double x_rd = x * (1 + k1 * r2 + k2 * pow(r2, 2) + k3 * pow(r2, 3));
    // double y_rd = y * (1 + k1 * r2 + k2 * pow(r2, 2) + k3 * pow(r2, 3));

    // 切向畸变 tangential distortion
    double x_td = 2 * p1 * x * y + p2 * (r2 + 2 * (x * x));
    // double y_td = 2 * p2 * x * y + p1 * (r2 + 2 * (y * y));

    return x_rd + x_td;
}

static double __correctY(double x, double y, const double coefficients[])
{
    double k1, k2, k3, p1, p2;
    k1 = coefficients[0];
    k2 = coefficients[1];
    k3 = coefficients[2];
    p1 = coefficients[3];
    p2 = coefficients[4];

    double r2 = x * x + y * y;

    // 径向畸变 radial distortion
    // double x_rd = x * (1 + k1 * r2 + k2 * pow(r2, 2) + k3 * pow(r2, 3));
    double y_rd = y * (1 + k1 * r2 + k2 * pow(r2, 2) + k3 * pow(r2, 3));

    // 切向畸变 tangential distortion
    // double x_td = 2 * p1 * x * y + p2 * (r2 + 2 * (x * x));
    double y_td = 2 * p2 * x * y + p1 * (r2 + 2 * (y * y));

    return y_rd + y_td;
}

void correctXY(double x, double y, double *x_correct, double *y_correct)
{
    *x_correct = __correctX(x, y, CoeffX);
    *y_correct = __correctY(x, y, CoeffY);
}
